#include <buola/cv/corientedchamfer.h>

#include <buola/image/algorithm/transform.h>
#include <buola/image/algorithm/edges.h>
#include <buola/image/algorithm/threshold.h>
#include <buola/image/algorithm/filter.h>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/internal.hpp>
#include <stack>
#pragma GCC diagnostic pop

namespace buola { namespace cvi {

void oriented_canny(const img::CView_gray8 &pSrc,const std::vector<const img::CMView_gray8*> &pDst,
                    double pLowThresh,double pHighThresh,int pApertureSize,int pOriBins)
{
//    float lGradientEpsilon=1e1f*sq(pApertureSize);

//     int i, j;
//CvMat mag_row;
    
    if(!(pApertureSize&1)||pApertureSize<3||pApertureSize>7)
        throw XArgument("oriented_canny: invalid aperture size");

    CSize_i lSize=pSrc.Size();

    img::CImage_gray16s lDX(lSize);
    img::CImage_gray16s lDY(lSize);

    img::sobel(pSrc,lDX,1,0,pApertureSize);
    img::sobel(pSrc,lDY,0,1,pApertureSize);

    int lLow=pLowThresh;
    int lHigh=pHighThresh;

    std::unique_ptr<char[]> lBuffer(new char[(lSize.x+2)*(lSize.y+2)+(lSize.x+2)*3*sizeof(int)]);

    int* lMagBuf[3];
    
    lMagBuf[0]=(int*)lBuffer.get();
    lMagBuf[1]=lMagBuf[0]+lSize.x+2;
    lMagBuf[2]=lMagBuf[1]+lSize.x+2;
    uint8_t *lMap=(uint8_t*)(lMagBuf[2]+lSize.x+2);
    int lMapStep=lSize.x+2;

    std::stack<uint8_t*> lStack;

    memset(lMagBuf[0],0,(lSize.x+2)*sizeof(int));
    memset(lMap,1,lMapStep*(lSize.y+2));

    /* sector numbers 
       (Top-Left Origin)

        1   2   3
         *  *  * 
          * * *  
        0*******0
          * * *  
         *  *  * 
        3   2   1
    */

    img::CImage_gray32f lDX32F(lSize);
    img::CImage_gray32f lDY32F(lSize);
    
    convert_scale(lDX,lDX32F);
    convert_scale(lDY,lDY32F);

    img::CImage_gray32f lOrient(lSize);

    cart_to_phase(lDX32F,lDY32F,lOrient);
    
    // calculate magnitude and angle of gradient, perform non-maxima supression.
    // fill the map with one of the following values:
    //   0 - the pixel might belong to an edge
    //   1 - the pixel can not belong to an edge
    //   2 - the pixel does belong to an edge
    for(int i=0;i<=lSize.y;i++)
    {
        int* lMag=lMagBuf[(i>0)+1]+1;
        const short *lDXPtr=lDX.RawData(0,i);
        const short *lDYPtr=lDY.RawData(0,i);
        int lPrevFlag=0;

        if(i<lSize.y)
        {
            lMag[-1]=lMag[lSize.x]=0;

            for(int j=0;j<lSize.x;j++)
            {
                lMag[j]=abs(lDXPtr[j])+abs(lDYPtr[j]);
            }
        }
        else
        {
            memset(lMag-1,0,(lSize.x+2)*sizeof(int));
        }

        // at the very beginning we do not have a complete ring
        // buffer of 3 magnitude rows for non-maxima suppression
        if(i==0)
            continue;

        uint8_t *lMapPtr=lMap+lMapStep*i+1;
        
        lMag=lMagBuf[1]+1; // take the central row
        lDXPtr=lDX.RawData(0,i-1);
        lDYPtr=lDY.RawData(0,i-1);
        
        int lMagStep1=(int)(lMagBuf[2]-lMagBuf[1]);
        int lMagStep2=(int)(lMagBuf[0]-lMagBuf[1]);
        
        for(int j=0;j<lSize.x;j++)
        {
            static constexpr int lCannyShift=15;
            static constexpr int lTG22=(int)(0.4142135623730950488016887242097*(1<<lCannyShift) + 0.5);

            int x=lDXPtr[j];
            int y=lDYPtr[j];
            int s=x^y;
            int m=lMag[j];

            x=abs(x);
            y=abs(y);

            bool lTest=false;
                
            if(m>lLow)
            {
                int tg22x=x*lTG22;
                int tg67x=tg22x+((x+x)<<lCannyShift);

                y <<= lCannyShift;

                if(y<tg22x)
                {
                    lTest=m>lMag[j-1]&&m>=lMag[j+1];
                }
                else if( y > tg67x )
                {
                    lTest=m>lMag[j+lMagStep2]&&m>=lMag[j+lMagStep1];
                }
                else
                {
                    s=s<0?-1:1;
                    lTest=m>lMag[j+lMagStep2-s]&&m>lMag[j+lMagStep1+s];
                }
            }

            if(lTest)
            {
                if(m>lHigh&&!lPrevFlag&&lMapPtr[j-lMapStep]!=2)
                {
                    lMapPtr[j]=2;
                    lStack.push(lMapPtr+j);
                    lPrevFlag=1;
                }
                else
                {
                    lMapPtr[j]=0;
                }
            }
            else
            {
                lPrevFlag=0;
            }
        }

        // scroll the ring buffer
        lMag=lMagBuf[0];
        lMagBuf[0]=lMagBuf[1];
        lMagBuf[1]=lMagBuf[2];
        lMagBuf[2]=lMag;
    }

    // now track the edges (hysteresis thresholding)
    
    auto check_and_push=[&lStack](uint8_t *m)
    {
        if(!*m)
        {
            *m=2;
            lStack.push(m);
        }
    };
    
    while(!lStack.empty())
    {
        uint8_t* m;

        m=lStack.top();
        lStack.pop();
    
        check_and_push(m-1);
        check_and_push(m+1);
        check_and_push(m-lMapStep-1);
        check_and_push(m-lMapStep);
        check_and_push(m-lMapStep+1);
        check_and_push(m+lMapStep-1);
        check_and_push(m+lMapStep);
        check_and_push(m+lMapStep+1);
    }

    uint8_t *lDstRow[pOriBins];
    for( int i = 0; i < lSize.y; i++ )
    {
        const uint8_t *lMapPtr = lMap + lMapStep*(i+1) + 1;
        const float* lOrientRow = lOrient.RawData(0,i);

        for( int j=0;j<pOriBins;j++)
        {
            lDstRow[j]=pDst[j]->RawData(0,i);
        }
        
        for( int j = 0; j < lSize.x; j++ )
        {
            if (lMapPtr[j]==2) // if it's an edge
            {
                int lChannel = (int)(lOrientRow[j]/(float(M_PI)/(float)pOriBins)) % pOriBins;
                lDstRow[lChannel][j]=(uint8_t)-1;
                lDstRow[(lChannel+1)%pOriBins][j] = (uint8_t)-1;
            }
        }
    }
}

COrientedChamfer::COrientedChamfer(const img::CView_gray8 &pIm)
{
    for(int i=0;i<8;i++)
    {
        mEdgesImage[i].Recreate(pIm.Size(),img::pixel(0));
        mDistImage[i].Recreate(pIm.Size());
        mOriImage[i].Recreate(pIm.Size());
    }

    std::vector<const img::CMView_gray8*> lEdgesViews;
    for(int i=0;i<8;i++)
    {
        lEdgesViews.emplace_back(&mEdgesImage[i]);
    }
    
    oriented_canny(pIm,lEdgesViews,50,200,3,8);
        
    for(int i=0;i<8;i++)
    {
        const auto &lView=mEdgesImage[i];
        for(int x=0;x<4;x++)
            for(int y=0;y<pIm.Height();y++)
                lView(x,y)=0;
        for(int x=4;x<pIm.Width()-4;x++)
        {
            for(int y=0;y<4;y++)
                lView(x,y)=0;
            for(int y=pIm.Height()-4;y<pIm.Height();y++)
                lView(x,y)=0;
        }
        for(int x=pIm.Width()-4;x<pIm.Width();x++)
            for(int y=0;y<pIm.Height();y++)
                lView(x,y)=0;
        
        img::threshold(lView,lView,2,255,cv::THRESH_BINARY_INV);
        
        dist_transform(mEdgesImage[i],mDistImage[i],img::DIST_L2,3,mOriImage[i]);
    }
}

float COrientedChamfer::GetDistance(const CPoint_d &pP,int i)
{
    //try bicubic interpolation
    if(pP.x<0||pP.y<0||pP.x>mEdgesImage[i].Width()-1||pP.y>mEdgesImage[i].Height()-1)
        return -1.0;

    auto lClosest=mOriImage[i](std::lrint(pP.x),std::lrint(pP.y));
    CPoint_d lPoint(lClosest[0],lClosest[1]);
    return dist(lPoint,pP);
}

CPoint_i COrientedChamfer::GetClosest(const CPoint_i &pP,int i)
{
    auto lClosest=mOriImage[i](std::lrint(pP.x),std::lrint(pP.y));
    return CPoint_i(lClosest[0],lClosest[1]);
}

/*namespace cvi*/ } /*namespace buola*/ } 
